#include "ArFilter.h"
#include <opencv2\imgproc\imgproc.hpp>
#include <QDebug>

ArFilter::ArFilter(const std::string &pattName) : Filter(1, 1), pattName(pattName)
{
	ARParam cparam, wparam;
	int res = arParamLoad("camera_para.dat", 1, &wparam);
	res = arParamChangeSize(&wparam, 640, 480, &cparam);

	res = arInitCparam(&cparam);
	res = arParamDisp(&cparam);

	pattId = arLoadPatt(pattName.c_str());

}


ArFilter::~ArFilter(void)
{
}

void ArFilter::Process()
{
	int thresh = 100;
	ARMarkerInfo *markerInfo;
	int markerNum;
	double pattCenter[2] = {0.0, 0.0};
	double pattWidth = 80.0;
	double pattTrans[3][4];

	cv::Mat image;
	cv::cvtColor(*inputs[0], image, CV_GRAY2BGRA);

	arDetectMarker(image.data, thresh, &markerInfo, &markerNum);
	qDebug() << "Marker num: " << markerNum;

	outputs[0] = cv::Mat(4, 4, CV_32FC1, cv::Scalar(0));

	if (markerNum)
	{
		//find best
		int best = -1;
		double bestcf = -1.0f;
		for (int i = 0; i < markerNum; ++i)
		{
			if (markerInfo[i].cf > bestcf && markerInfo[i].id == pattId)
			{
				bestcf = markerInfo[i].cf;
				best = i;
			}
		}

		if (best >= 0)
		{
			qDebug() << QString::fromStdString(pattName);
			qDebug() << "CF: " << bestcf;
			arGetTransMat(&markerInfo[best], pattCenter, pattWidth, pattTrans);
			qDebug() << pattTrans[0][0] << "\t" << pattTrans[0][1] << "\t" << pattTrans[0][2] << "\t" << pattTrans[0][3];
			qDebug() << pattTrans[1][0] << "\t" << pattTrans[1][1] << "\t" << pattTrans[1][2] << "\t" << pattTrans[1][3];
			qDebug() << pattTrans[2][0] << "\t" << pattTrans[2][1] << "\t" << pattTrans[2][2] << "\t" << pattTrans[2][3];

			outputs[0] = cv::Mat(4, 4, CV_32FC1);
		
			for (int i = 0; i < 3; ++i)
			{
				float *p = outputs[0].ptr<float>(i);
				for (int j = 0; j < 4; ++j)
				{
					p[j] = pattTrans[i][j];
				}
			}
			outputs[0].at<float>(3,3) = 1.0f;

		}
	}

	
}
